#include <cmath>
#include <string>

#include "utilities.h"

void printVector3(Vector3 v, std::string s) {
	std::cout << "Vector " << s << ": " << std::endl;
	std::cout << v.x << ", " << v.y << ", " << v.z << std::endl;
}

void printMatrix(Matrix m, std::string s) {
	std::cout << "Matrix " << s << ": " << std::endl;
	std::cout << m.m0 << ", " << m.m4 << ", " << m.m8 << ", " << m.m12 << std::endl;
	std::cout << m.m1 << ", " << m.m5 << ", " << m.m9 << ", " << m.m13 << std::endl;
	std::cout << m.m2 << ", " << m.m6 << ", " << m.m10 << ", " << m.m14 << std::endl;
	std::cout << m.m3 << ", " << m.m7 << ", " << m.m11 << ", " << m.m15 << std::endl;
}

Vector Vector::CrossProduct(Vector v) {
	Vector result = { y * v.z - z * v.y,
	                  z * v.x - x * v.z,
	                  x * v.y - y * v.x
	                };
	return result;
}

float Vector::Length() {
	float result = sqrtf(x * x + y * y + z * z);

	return result;
}

Vector Vector::Normalize() {
	Vector result = { x, y, z };

	float length = sqrtf(x * x + y * y + z * z);
	if (length == 0.0f) length = 1.0f;
	float ilength = 1.0f / length;

	result.x *= ilength;
	result.y *= ilength;
	result.z *= ilength;

	return result;
}

Vector Vector::Subtract(Vector v) {
	Vector result = { x - v.x, y - v.y, z - v.z };

	return result;
}

Vector Vector::Add(Vector v) {
	Vector result = { x + v.x, y + v.y, z + v.z };

	return result;
}

Vector Vector::Scale(float scalar) {
	Vector result = { x * scalar, y * scalar, z * scalar };

	return result;
}

std::pair<Vector3, Vector3> getOtherTwoAxis(Vector3 axis) {
	Vector3 firstAxis = Vector3CrossProduct(axis, {1, 0, 0});
	if (firstAxis.x == 0 && firstAxis.y == 0 && firstAxis.z == 0) {
		firstAxis = Vector3CrossProduct(axis, {0, 1, 0});
	}
	Vector3 secondAxis = Vector3CrossProduct(axis, firstAxis);
	firstAxis = Vector3Normalize(firstAxis); // 地球X轴基向量
	secondAxis = Vector3Normalize(secondAxis); // 地球Y轴基向量
	return {firstAxis, secondAxis};
}

std::vector<Vector3> getCirclePointsPos(Vector3 circlePos, Vector3 normal, float radius, uint16_t pointsCount) {
	std::vector<Vector3> res;
	auto [a, b] = getOtherTwoAxis(normal);
	float deltAngle = 360 / pointsCount;
	float alphaDeg = 0;
	for (int i = 0; i < pointsCount; ++i) {
		Vector3 pointPos;
		float alpha = DEG2RAD * alphaDeg;
		pointPos.x = circlePos.x + radius * a.x * cosf(alpha) + radius * b.x * sinf(alpha);
		pointPos.y = circlePos.y + radius * a.y * cosf(alpha) + radius * b.y * sinf(alpha);
		pointPos.z = circlePos.z + radius * a.z * cosf(alpha) + radius * b.z * sinf(alpha);
		alphaDeg += deltAngle;
		res.push_back(pointPos);
	}
	return res;
}

std::vector<Vector3> getCirclePointsPos(Vector3 circlePos, Vector3 normalOne,  Vector3 normalTwo, float radius, uint16_t pointsCount) {
	std::vector<Vector3> res;
	float deltAngle = 360 / pointsCount;
	float alphaDeg = 0;
	for (int i = 0; i < pointsCount; ++i) {
		Vector3 pointPos;
		float alpha = DEG2RAD * alphaDeg;
		pointPos.x = circlePos.x + radius * normalOne.x * cosf(alpha) + radius * normalTwo.x * sinf(alpha);
		pointPos.y = circlePos.y + radius * normalOne.y * cosf(alpha) + radius * normalTwo.y * sinf(alpha);
		pointPos.z = circlePos.z + radius * normalOne.z * cosf(alpha) + radius * normalTwo.z * sinf(alpha);
		alphaDeg += deltAngle;
		res.push_back(pointPos);
	}
	return res;
}

